#ifndef PATHSCONTROL_H_
#define PATHSCONTROL_H_


#include <ros/ros.h>
#include <vector>
#include <queue>
#include <utility>
#include <iostream>
#include <memory>
#include <string>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseArray.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <glog/logging.h>
#include <thread>
#include "mpc_controller.h"
// #include "lqr_controller.h"

using std::string;

class Paths_Control
{
public:
    Paths_Control();
    ~Paths_Control();

    void Trajectory(const nav_msgs::Path::ConstPtr &trajectory_msg);
    void Speed(const geometry_msgs::PoseArray::ConstPtr &speed_msg);
    void Acceleration(const geometry_msgs::PoseArray::ConstPtr &acceleration_msg);
    void Theta(const geometry_msgs::PoseArray::ConstPtr &theta_msg);
    void Kappas(const geometry_msgs::PoseArray::ConstPtr &kappas_msg);
    void Odom(const nav_msgs::Odometry &msg);
    void MoveBaseSimpleGoal(const geometry_msgs::PoseStamped::ConstPtr &move_base_goal_msg);
    void Start_Dynamic(const std_msgs::String::ConstPtr &start_msg);
    bool Vehical_Stop(const geometry_msgs::Pose &goal_point, nav_msgs::Odometry &odom);
    void public_control_thread();

private:

    string Start_local_planning;
    bool is_move_base_simple_goal = false;
    bool is_vehical_stop_set = true;

    ros::NodeHandle nh;

    ros::Publisher pub_speed_control;

    ros::Subscriber sub_speed;
    ros::Subscriber sub_acceleration;
    ros::Subscriber sub_trajectory;
    ros::Subscriber sub_theta;
    ros::Subscriber sub_kappas;
    ros::Subscriber sub_Odom;
    ros::Subscriber sub_start_dynamic;
    ros::Subscriber sub_MoveBaseSimpleGoal;

    nav_msgs::Odometry odom_;
    nav_msgs::Path loacl_paths;
    geometry_msgs::Pose goal_pose_;

    std::vector<double> car_speed;
    std::vector<double> car_acceleration;
    std::vector<double> car_theta;
    std::vector<double> car_kappas;

    std::unique_ptr<std::thread> public_control_thread_;
    // LqrController Lqr;
    MpcController Mpc;

};

#endif // PATHSCONTROL_H_